#include "common_inc.h"
#include "stm32f4xx_hal.h"
#include "stm32f4xx_hal_flash_ex.h"
#include "user_define.h"
#include "usart.h"
#include "CircleBuffer.h"
#include "bsp_i2c_ds3231.h"
#include "w25qxx.h"

extern DummyRobot dummy;
extern bool startFlag,StopAndBackFlag;
extern FLASH_ProcessTypeDef pFlash;
extern uint8_t AdjustPlanstep;

extern StreamSink *DrawJointAndPosOutputPtr;
extern bool DrawJointAndPosFlag,PrintBatMsgFlag;
extern uint16_t DrawGapMs;
extern bool FindHomeFlagUpdataFlag;
extern NEED_FIND_HOME_FLAG NeedFindHomeFlag;
extern DOF6Kinematic::Joint6D_t dynamicJointSpeedsWhenStop,JointValueWhenStop,targetJointsWhenStopOrDisable;

enum StepsType{
    POSITION_STEP = 1,
    HOLD_TIME_STEP,
    HAND_POWER_STEP,
    WAITING_COLOR_STEP,
    WAITING_CONTINUE_COMMAND_STEP,
};

struct StepsInfo{
    StepsType stepType;
    uint8_t spaceUsedSize;
    uint16_t stepSize;
    uint16_t currentStepIdx;
    uint16_t ReDoTimes;
};


struct HandPowerStep{
    float handPowerValue;
    float JoinSpeedLimit;
};

struct WaitColorStep{
    uint16_t r;
    uint16_t g;
    uint16_t b;
};

enum WaitSignalStep{
    WAIT_START_SIG = 1,
    WAIT_NEXT_STEP_SIG,
};

struct WriteDanceInfo{
    uint32_t currentStepNum;
}writeDanceInfo;

struct DanceHandInfo{
    uint32_t StepSum;
}danceHandInfo;



struct AngleStepType{
    float j[6];
    float speedLimit;
    HandConMode handConMode;
    float handConValue;
}angleStep;

struct PositionStepType{
    float p[6];
    float speedLimit;
    HandConMode handConMode;
    float handConValue;
}positionStep;

#if defined(IMU6_T) || defined(IRC_T)
float Motor_Speed = MotorSpeed*1.0f;
void SetMotorAngle(uint8_t _id,const uint8_t mode,float _angle,float motorspeede){
    CAN_TxHeaderTypeDef txHeader;

    static uint8_t CanBuf[8] ;
    txHeader.StdId = _id << 7 | mode;

    txHeader.ExtId = 0;
    txHeader.IDE = CAN_ID_STD;
    txHeader.RTR = CAN_RTR_DATA;
    txHeader.DLC = 8;
    txHeader.TransmitGlobalTime = DISABLE;
    if(mode == 1 || mode == 0x50 ){
       CanBuf[0] = (int32_t)_angle;
    }else {
        auto *angle = (unsigned char *)&_angle;
        auto *speed = (unsigned char *)&motorspeede;
        for(int i = 0;i<4;i++){
            CanBuf[i] = *(angle+i);
            CanBuf[i+4] = *(speed+i);
        }
    }




    CanSendMessage(get_can_ctx(&hcan1), CanBuf, &txHeader);

}
#endif


void HomeKeyFunPhysical(){
    char p;
    HomeKeyFun(&p,0);
}


void HomeKeyFun(const char *_cmd, size_t _len){
    if(RobotSta == ROBOT_POWER_ON || RobotSta == ROBOT_SHUTDOWN || RobotSta == ROBOT_SHUTDOWN_ING){
        WriteLogToBuff(TYPE_C_COMMAND_LOG,"getted shutup command by Multi-function key");
        RobotShutup();
    }
    else if(dummy.HarVer == DUMMY){
        if(startFlag == false){
            dummy.SetEnable(true);
            startFlag = true;
            AdjustPlanstep = 0;
            RobotSta = ROBOT_WORK_WITH_PROCEDRUE;
            Respond(*usbStreamOutputPtr, "start\n");
            WriteLogToBuff(TYPE_C_COMMAND_LOG,"getted work command by Multi-function key");
        }else if(AdjustPlanstep > 1){
            if(AdjustPlanstep > 4 && AdjustPlanstep <= 26)
                StopAndBackFlag = true;

            if(AdjustPlanstep <= 3)
                AdjustPlanstep = 41;
            else if(AdjustPlanstep <= 7)
                AdjustPlanstep = 37;
            else if(AdjustPlanstep <= 10)
                AdjustPlanstep = 35;
            else if(AdjustPlanstep <= 24)
                AdjustPlanstep = 35;
            else if(AdjustPlanstep <= 25)
                AdjustPlanstep = 34;
            else if(AdjustPlanstep <= 26)
                AdjustPlanstep = 33;

            dummy.commandMode = DummyRobot::COMMAND_TARGET_POINT_SEQUENTIAL;


            Respond(*usbStreamOutputPtr, "stop\n");
        }
    }else if(RobotSta == ROBOT_CONTROL_WITH_COMMAND){
        RobotWorkWithProcedrue();
    }else if(RobotSta == ROBOT_WORK_WITH_PROCEDRUE){
        RobotEmeraencyStop();
    }else if(RobotSta == ROBOT_NORMAL_STOPED || RobotSta == ROBOT_EMERGENCY_STOPED){
        RobotRecover();
    }
}

void RestoreFromStopOrDisable(){
        RobotSta = RobotStaWhenStopOrDisable;
        dummy.SetJointSpeed(dummy.HarVer != BATTLE_CHARGE_THERE_AXIS?10:0.5);
        dummy.MoveJ(targetJointsWhenStopOrDisable.a[0],targetJointsWhenStopOrDisable.a[1],targetJointsWhenStopOrDisable.a[2],targetJointsWhenStopOrDisable.a[3],targetJointsWhenStopOrDisable.a[4],targetJointsWhenStopOrDisable.a[5]);
        //dummy.targetJoints = targetJointsWhenStopOrDisable;
        /*for (int i = 0; i < dummy.RobotAxisNum; ++i) {
            if(dummy.HarVer != BATTLE_CHARGE_THERE_AXIS && dummy.dynamicJointSpeeds.a[i] < 5)
                dummy.dynamicJointSpeeds.a[i] = 5;
            else if(dummy.HarVer == BATTLE_CHARGE_THERE_AXIS && dummy.dynamicJointSpeeds.a[i] < 0.1)
                dummy.dynamicJointSpeeds.a[i] = 0.1;
            //dummy.dynamicJointSpeeds.a[i] = fabs(dummy.currentJoints.a[i] - dummy.targetJoints.a[i])*(dummy.HarVer!=BATTLE_CHARGE_THERE_AXIS?10:10);
        }*/
        printf("robot shutup,sp:%.2f %.2f %.2f %.2f %.2f %.2f\n",dummy.dynamicJointSpeeds.a[0],dummy.dynamicJointSpeeds.a[1],dummy.dynamicJointSpeeds.a[2],dummy.dynamicJointSpeeds.a[3],dummy.dynamicJointSpeeds.a[4],dummy.dynamicJointSpeeds.a[5]);
}

void RobotRecover(){
    if(RobotSta == ROBOT_NORMAL_STOPED || RobotSta == ROBOT_EMERGENCY_STOPED){
        if(RobotSta != RobotStaWhenStopOrDisable) {
            RestoreFromStopOrDisable();
        }
    }else if(RobotSta == ROBOT_DISABLED){
        RobotRecoverProcessStep = 0;
        RobotSta = ROBOT_RECOVER_FORM_DISABLE;
    }
    printf("recover\n");
}

void RobotShutup(){
    if(RobotSta == ROBOT_POWER_ON || RobotSta == ROBOT_SHUTDOWN || RobotSta == ROBOT_SHUTDOWN_ING){
        RobotSta = ROBOT_CALIBRATING_MOTOR;
        printf("cali\n");
    }else if(RobotSta == ROBOT_DISABLED){
        RobotRecover();
    }
}

void RobotDisable(){
    if(RobotSta != ROBOT_DISABLED){
        if(RobotSta != ROBOT_NORMAL_STOPED && RobotSta != ROBOT_EMERGENCY_STOPED){
            for (int i = 0; i < 6; ++i) {
                dynamicJointSpeedsWhenStop.a[i] = dummy.dynamicJointSpeeds.a[i];
                RobotStaWhenStopOrDisable = RobotSta;
            }
            targetJointsWhenStopOrDisable = dummy.targetJoints;
        }
        RobotSta = ROBOT_DISABLED;
        printf("robot disable\n");
    }
}

void RobotNormalStop(){
    if((RobotSta == ROBOT_CONTROL_WITH_COMMAND || RobotSta == ROBOT_WORK_WITH_PROCEDRUE) && RobotSta != ROBOT_NORMAL_STOPED){
        for (int i = 0; i < 6; ++i) {
            dynamicJointSpeedsWhenStop.a[i] = dummy.dynamicJointSpeeds.a[i];
            RobotStaWhenStopOrDisable = RobotSta;
        }
        targetJointsWhenStopOrDisable = dummy.targetJoints;
        RobotSta = ROBOT_NORMAL_STOPED;
        RobotNormalStopStep = 0;
        printf("robot normal stop\n");
    }
}

void RobotShutdown(){
    if(NeedShutDownFlag == false && RobotSta != ROBOT_SHUTDOWN && RobotSta != ROBOT_SHUTDOWN_ING){
        if(RobotSta == ROBOT_NORMAL_STOPED || RobotSta == ROBOT_EMERGENCY_STOPED || RobotSta == ROBOT_DISABLED)
            RobotRecover();
        NeedShutDownFlag = true;
        RobotShutdownProcessStep = 0;
        printf("robot need shutdwon\n");
    }
}

void RobotWorkWithProcedrue(){
    if(IfAllMotorInitPos() == true && RobotSta != ROBOT_WORK_WITH_PROCEDRUE){
        if(RobotSta == ROBOT_CONTROL_WITH_COMMAND){
            RobotSta = ROBOT_WORK_WITH_PROCEDRUE;
        }else if(RobotSta == ROBOT_NORMAL_STOPED || RobotSta == ROBOT_EMERGENCY_STOPED){
            RobotRecover();
        }
        printf("robot work\n");
    }
}

void RobotEmeraencyStop(){
    if((RobotSta == ROBOT_CONTROL_WITH_COMMAND || RobotSta == ROBOT_WORK_WITH_PROCEDRUE) && RobotSta != ROBOT_EMERGENCY_STOPED){
        for (int i = 0; i < 6; ++i) {
            dynamicJointSpeedsWhenStop.a[i] = dummy.dynamicJointSpeeds.a[i];
            RobotStaWhenStopOrDisable = RobotSta;
        }
        targetJointsWhenStopOrDisable = dummy.targetJoints;
        RobotSta = ROBOT_EMERGENCY_STOPED;
        printf("robot emergency stop\n");
    }

}

#if defined(IMU6_T) || defined(IRC_T)
    float ZeroData[5] = {-0.475176f,0.0f,0.0f};
    uint8_t  ReadPosFLG = 0;
    uint8_t  IMU6_Work = 0;
    extern uint8_t SetPoint_Xnum ,SetPoint_Disknum ;
extern uint16_t GetColorIndex;
extern Driver_Status USBCmd_Gripers,USBCmd_Lifting ;
extern void CheckPlaneData(uint8_t Msg_Type,uint16_t param1,uint8_t check_cmd,uint8_t ask_result,uint16_t datalength,uint8_t takeon);
extern uint8_t  irc_dir;
#endif
void ParseCommandFirst(const char *_cmd, size_t _len, StreamSink &_responseChannel){
    /*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
#if defined(IMU6_T) || defined(IRC_T)
     if(_cmd[0] == '!'){
        if(strstr(&_cmd[1],"M[J]") != NULL) {
            uint8_t argNum,id;
            argNum = sscanf(_cmd, "! M[J] %d", &id);
            Respond(*usbStreamOutputPtr,"argNum = %d,id=%d\n",argNum,id);
            if(argNum == 1){
                if(id == 1){
                    Motor_Speed = Motor_Speed/100;
                }
                else if(id == 5){
                    Motor_Speed = Motor_Speed/100;
                }
                else {
                    Motor_Speed = Motor_Speed;
                }
                SetMotorAngle(id,0x02,0.0f,Motor_Speed);
            }
            // Float to Bytes

        }
         if(strstr(&_cmd[1],"Set Pos:") != NULL) {
             uint8_t argNum;
             float Value[6];
             argNum = sscanf(_cmd, "! Set Pos:%f,%f,%f,%f,%f,%f",&Value[0],&Value[1],&Value[2],&Value[3],&Value[4],&Value[5]);
             Respond(*usbStreamOutputPtr,"argNum = %d,1:%f,2:%f,3:%f,4:%f,5:%f,6:%f\n",argNum,Value[0],Value[1],Value[2],Value[3],Value[4],Value[5]);
             if(argNum == 3){
                  for(int i=0;i<argNum;i++){
                      if(i == 0){
                          Motor_Speed = Motor_Speed/100;
                      }
                      else if(i == 4){
                          Motor_Speed = Motor_Speed/100;
                      }
                      else {
                          Motor_Speed = Motor_Speed;
                      }
                      SetMotorAngle(i+1,0x08,Value[i],Motor_Speed);
                  }
             }
         }
         if(strstr(&_cmd[1],"M[A]") != NULL) {
             uint8_t argNum,id=0;
             float Value;
             argNum = sscanf(_cmd, "! M[A] %d,%f", &id,&Value);
             Respond(*usbStreamOutputPtr,"argNum = %d,id=%d,CharngData = %f,ZeroData[%d-1] = %f\n",argNum,id,Value,id,ZeroData[id-1]);
             if(argNum == 2){
                     if(id == 1){
                         Motor_Speed = Motor_Speed/2;
                     }
                     else if(id == 5){
                         Motor_Speed = Motor_Speed;
                     }
                     else {
                         Motor_Speed = Motor_Speed;
                     }
                     ZeroData[id-1] =  ZeroData[id-1]+(Value*1.0f);
                     SetMotorAngle(id,0x08,ZeroData[id-1],Motor_Speed);

             }
         }
         if(strstr(&_cmd[1],"Read Pos?") != NULL) {
             ReadPosFLG = 1;
             for(int i = 0;i<6;i++){
                 SetMotorAngle(i+1,0x09,0,Motor_Speed);
             }

         }
         if(strstr(&_cmd[1],"Open Grippers?") != NULL){
             USBCmd_Gripers = Open;
         }
         if(strstr(&_cmd[1],"Close Grippers?") != NULL){
             USBCmd_Gripers = Close;
         }
         if(strstr(&_cmd[1],"Up Lifting?") != NULL){
             USBCmd_Lifting = Up;
         }
         if(strstr(&_cmd[1],"Down Lifting?") != NULL){
             USBCmd_Lifting = Down;
         }
         if(strstr(&_cmd[1],"Set X Point") != NULL){
             uint8_t argNum,id;
             argNum = sscanf(_cmd, "! Set X Point:%d", &id);
             Respond(*usbStreamOutputPtr,"X Point:%d\n",id);
             if(argNum == 1){
                 SetPoint_Xnum = id;
             }

         }

         if(strstr(&_cmd[1],"Set Disk Point") != NULL){
             uint8_t argNum,id;
             argNum = sscanf(_cmd, "! Set Disk Point:%d", &id);
             Respond(*usbStreamOutputPtr,"Disk Point:%d\n",id);
             if(argNum == 1){
                 SetPoint_Disknum = id;
             }

         }

         if(strstr(&_cmd[1],"Check System Version!")!= NULL){
             PrintSysVerMsg();
         }
         if(strstr(&_cmd[1],"Read Signal:") != NULL) {
             uint8_t argNum,id=0;
             argNum = sscanf(_cmd, "! Read Signal:%d", &id);
             Respond(*usbStreamOutputPtr,"argNum = %d,id=%d\n",argNum,id);
             if(argNum == 1){
                 if(id == 1){
                     Motor_Speed = Motor_Speed/100;
                 }
                 else if(id == 5){
                     Motor_Speed = Motor_Speed/100;
                 }
                 else {
                     Motor_Speed = Motor_Speed;
                 }
                 SetMotorAngle(id,0x50,0,Motor_Speed);
             }

         }
         if(strstr(&_cmd[1],"Start Work") != NULL) {
             if(IMU6_Work == 0){
                 IMU6_Work = 1;
             }

             //SetMotorAngle(1,0x51,0,Motor_Speed);

         }
         if(strstr(&_cmd[1],"Stop Work") != NULL){
             if(IMU6_Work == 1){
                 IMU6_Work = 0;
             }
         }
         if(strstr(&_cmd[1],"Read Color?") != NULL) {
             SetMotorAngle(3,0x51,0,0);
            #ifdef IMU6_T
             GetColorIndex++;
            #endif
         }
         if(strstr(&_cmd[1],"ENABLE") != NULL){
             uint8_t argNum,id;
             argNum = sscanf(_cmd, "! ENABLE %d", &id);
             Respond(*usbStreamOutputPtr,"argNum = %d,ENABLE M[%d]\n",argNum,id);
             SetMotorAngle(id,0x01,1,Motor_Speed);
         }
    }
#else
    if (_cmd[0] == '!' || !dummy.IsEnabled()) {
        std::string s(_cmd);
        if (s.find("STOP") != std::string::npos) {
            dummy.commandHandler.EmergencyStop();
            Respond(_responseChannel, "Stopped ok");
        } else if (s.find("START") != std::string::npos) {
            dummy.SetEnable(true);
            Respond(_responseChannel, "Started ok");
        } else if (s.find("DISABLE") != std::string::npos) {
            RobotSta = ROBOT_DISABLED;
            //ThreadControlLoopFixUpdateStep = 0;
            Respond(_responseChannel, "Disabled ok");
        }
    }else if (_cmd[0] == '>' || _cmd[0] == '@' || _cmd[0] == '&' || _cmd[0] == '*' || _cmd[0] == 'M' ) {
        uint32_t freeSize = dummy.commandHandler.Push(_cmd);
        if(freeSize == 0xFF)
            Respond(_responseChannel, "Err:MessageQueue space is full!\n");
    }

    if(_cmd[0] == '>' || _cmd[0] == '@' || _cmd[0] == '*')
        RobotSta = ROBOT_CONTROL_WITH_COMMAND;


    //'#'
    if (_cmd[0] == '#') {
        std::string s(_cmd);
        if (s.find("GETJSTATE") != std::string::npos) {
            Respond(_responseChannel, "\nMode:%s",cmdModeNames[RobotSta]);
            Respond(_responseChannel, "\nTarAngle: >%.4f, %.4f, %.4f,",
                    dummy.targetJoints.a[0], dummy.targetJoints.a[1],dummy.targetJoints.a[2]);
            Respond(_responseChannel, "%.4f, %.4f, %.4f",
                    dummy.targetJoints.a[3],dummy.targetJoints.a[4], dummy.targetJoints.a[5]);



            Respond(_responseChannel, "\nCurAngle: >%.4f,%.4f,%.4f,",
                    dummy.currentJoints.a[0], dummy.currentJoints.a[1],dummy.currentJoints.a[2]);

            Respond(_responseChannel, "%.4f,%.4f,%.4f",
                    dummy.currentJoints.a[3],dummy.currentJoints.a[4], dummy.currentJoints.a[5]);

            Respond(_responseChannel, "\nCurPose6D: @%.4f,%.4f,%.4f,",
                    dummy.currentPose6D.X,dummy.currentPose6D.Y,dummy.currentPose6D.Z);
            Respond(_responseChannel, "%.4f,%.4f,%.4f",
                    dummy.currentPose6D.A,dummy.currentPose6D.B,dummy.currentPose6D.C);

            Respond(_responseChannel, "\nTarPose6D: @%.4f,%.4f,%.4f,",
                    dummy.targetPose6D.X,dummy.targetPose6D.Y,dummy.targetPose6D.Z);
            Respond(_responseChannel, "%.4f,%.4f,%.4f",
                    dummy.targetPose6D.A,dummy.targetPose6D.B,dummy.targetPose6D.C);

            Respond(_responseChannel,"\nMotor Init:%d %d %d %d %d %d",InitPosFalg[1],InitPosFalg[2],InitPosFalg[3],InitPosFalg[4],InitPosFalg[5],InitPosFalg[6]);
                    Respond(_responseChannel, "\nJionsState: %d,%d,%d,%d,%d,%d",(dummy.jointsStateFlag & (1 << 1))==0?0:1,(dummy.jointsStateFlag & (1 << 2))==0?0:1,(dummy.jointsStateFlag & (1 << 3))==0?0:1,
                    (dummy.jointsStateFlag & (1 << 4))==0?0:1,(dummy.jointsStateFlag & (1 << 5))==0?0:1,(dummy.jointsStateFlag & (1 << 6))==0?0:1);

                    Respond(_responseChannel,"\n%d-%02d-%02d %02d:%02d:%02d week:%d temp:%d\n",calendar.year,calendar.month,calendar.day,calendar.hour,calendar.min,calendar.sec,calendar.week,calendar.temperature);//打印到串口屏不能有printf("\n");换行！！！

        }else if (s.find("CMDMODE") != std::string::npos) {
            uint32_t mode;
            sscanf(_cmd, "#CMDMODE %lu", &mode);
            dummy.SetCommandMode(mode);
            Respond(_responseChannel, "Set command mode to [%lu]", mode);
        }
        else if (s.find("SYS VER") != std::string::npos) {
            PrintSysVerMsg();
        }
        else if (s.find("Robot shutdown") != std::string::npos) {
            sprintf(SprintfBuff,"getted shutdown command by ");
//            switch (_responseChannel) {
//                case usb:
//
//                    break;
//
//            }
//            sprintf(&SprintfBuff(strlen(SprintfBuff)),"%s",);
            WriteLogToBuff(TYPE_C_COMMAND_LOG,"getted shutdown command by Type-C");
            RobotShutdown();
        }
        else if (s.find("Robot shutup") != std::string::npos) {
            WriteLogToBuff(TYPE_C_COMMAND_LOG,"getted shutup command by Type-C");
            RobotShutup();
        }else if (s.find("Robot normal stop") != std::string::npos) {
            WriteLogToBuff(TYPE_C_COMMAND_LOG,"getted normal stop command by Type-C");
            RobotNormalStop();
        }else if (s.find("Robot emergency stop") != std::string::npos) {
            WriteLogToBuff(TYPE_C_COMMAND_LOG,"getted Robot emergency stop command by Type-C");
            RobotEmeraencyStop();
        }else if (s.find("Robot disable") != std::string::npos) {
            WriteLogToBuff(TYPE_C_COMMAND_LOG,"getted Robot disable command by Type-C");
            RobotDisable();
        }else if (s.find("Robot work") != std::string::npos) {
            WriteLogToBuff(TYPE_C_COMMAND_LOG,"getted Robot work command by Type-C");
            RobotWorkWithProcedrue();
        }

    }
    //'M'
    else if(_cmd[0] == 'M') {
        if (strstr(&_cmd[3], "[O]") != NULL) {
            int id;
            float val;
            float value[6];
            uint8_t argNum;

            argNum = sscanf(_cmd, "MA [O] %f,%f,%f,%f,%f,%f", &value[0], &value[1], &value[2], &value[3],
                            &value[4], &value[5]);
            if (argNum == 6) {
                for (int i = 0; i < 6; i++) {
                    dummy.initPose.a[i] = value[i];
                }
                sprintf(SprintfBuff,"Alter origin:%.3f,%.3f,%.3f,%.3f,%.3f,%.3f -> %.3f,%.3f,%.3f,%.3f,%.3f,%.3f",dummy.initPose.a[0],dummy.initPose.a[1],dummy.initPose.a[2],
                        dummy.initPose.a[3],dummy.initPose.a[4],dummy.initPose.a[5],value[0],value[1],value[2],value[3],value[4],value[5]);
                WriteLogToBuff(ROBOT_PARAMETER_ALTER_LOG,SprintfBuff);

                Respond(_responseChannel, "Motor all home set to %.3f,%.3f,%.3f,%.3f,%.3f,%.3f\n", argNum, value[0],
                        value[1], value[2], value[3], value[4], value[5]);
            } else {
                argNum = sscanf(_cmd, "MA [O] %d,%f", &id, &val);
                if (argNum == 2 && id > 0 && id <= 6) {
                    if (abs(val) < 10) {
                        dummy.initPose.a[id - 1] += val;
                        Respond(_responseChannel, "M%d initPose add %.3f %.3f\n", id, val,dummy.initPose.a[id - 1]);
                    }
                } else {
                    argNum = sscanf(_cmd, "MA [O] %d", &id);
                    if (argNum == 1 && id >= 0 && id <= 6) {
                       if(id == 0){
                            UserDataU.UserData.JointInitPose[0] = dummy.initPose.a[0];
                            UserDataU.UserData.JointInitPose[1] = dummy.initPose.a[1];
                            UserDataU.UserData.JointInitPose[2] = dummy.initPose.a[2];
                            UserDataU.UserData.JointInitPose[3] = dummy.initPose.a[3];
                            UserDataU.UserData.JointInitPose[4] = dummy.initPose.a[4];
                            UserDataU.UserData.JointInitPose[5] = dummy.initPose.a[5];
                        }else{
                           if (strstr(&_cmd[3], " encoder") != NULL) {
                                dummy.motorJ[id]->ApplyPositionAsHome();
                           }else
                               UserDataU.UserData.JointInitPose[id-1] = dummy.initPose.a[id-1];
                        }
                        UserDataU.UserData.reserved0[0] = id;
                        WriteUserDataToFalsh();
                        Respond(_responseChannel, "set home pos success\n", id);

                    } else
                        Respond(_responseChannel, "initPose:%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\n", dummy.initPose.a[0],
                                dummy.initPose.a[1], dummy.initPose.a[2], dummy.initPose.a[3], dummy.initPose.a[4],
                                dummy.initPose.a[5]);
                }
            }


        }else if (strstr(&_cmd[2], "[J]") != NULL) {
            int id;
            uint8_t argNum;

            argNum = sscanf(_cmd, "M [J] %d", &id);
            Respond(_responseChannel, "Adjust M %d\n", id);
            if (argNum == 1 && id > 0 && id <= 6) {
                dummy.motorJ[id]->DoCalibration();
            }
        }else if (strstr(&_cmd[2], "[I]") != NULL) {
            int id;
            uint8_t argNum,Type,UpdataFlag;

            argNum = sscanf(_cmd, "M [I] id:%d T:%d U:%d", &id,&Type,&UpdataFlag);
            Respond(_responseChannel, "argNum:%d Init M id:%d T:%d U:%d\n", argNum,id,Type,UpdataFlag);
            if (argNum == 3 && id >= 0 && id <= dummy.RobotAxisNum && (Type == FIND_HOME_WITH_PHYSICAL_LIMIT || Type == FIND_HOME_WITH_PHYSICAL_KEY)) {
                NeedFindHomeFlag = (NEED_FIND_HOME_FLAG)Type;
                FindHomeFlagUpdataFlag = UpdataFlag==0? false: true;
                for (int i = 1; i <= dummy.RobotAxisNum; ++i) {
                    if(id == i || id == 0){
                        if(Type == FIND_HOME_WITH_PHYSICAL_LIMIT)
                            dummy.motorJ[i]->FindHomePosWithVelocity(FIND_INIT_POS_SPEED*(dummy.motorJ[i]->inverseDirection?1:-1)*(dummy.PHYSICAL_LIMIT_DIRT[i]?-1:1),FindHomeFlagUpdataFlag,NeedFindHomeFlag);
                        else if(Type == FIND_HOME_WITH_PHYSICAL_KEY)
                            dummy.motorJ[i]->FindHomePosWithVelocity(FIND_INIT_POS_SPEED*(dummy.motorJ[i]->inverseDirection?1:-1)*(dummy.KEY_DETECT_DIRT[i]?-1:1),FindHomeFlagUpdataFlag,NeedFindHomeFlag);
                    }

                }


            }
        }else if(strstr(&_cmd[3],"[PIDV]") != NULL) {
            uint8_t argNum;
            int Id, P, I, D, V;
            argNum = sscanf(_cmd, "M%d [PIDV] %d, %d, %d, %d", &Id, &P, &I, &D, &V);
            if (argNum == 5 && Id >0 && Id <= 6) {
                //Respond(*usbStreamOutputPtr, "set Id:%d P:%d I:%d D:%d V:%d\n", Id, P, I, D, V);
                dummy.motorJ[Id]->SetDceKpKiKdKv(P, I, D, V);

            }else{
                argNum = sscanf(_cmd, "M%d [PIDV] ?", &Id);
                if (argNum == 1 && Id >=0 && Id <= 6){
                    if(Id == 0){
                        for (int i = 1; i <= 6; ++i) {
                            dummy.motorJ[i]->SetDceKpKiKdKv(0,0,0,0);
                            osDelay(10);
                        }
                    }else
                        dummy.motorJ[Id]->SetDceKpKiKdKv(0,0,0,0);
                }else{
                    Respond(*usbStreamOutputPtr, "err!\n");

                }
            }



        }
    }

    //'D'
    else if(memcmp(_cmd,"[D]",3) == 0){
        if(_cmd[3] == 'W'){
            if(_cmd[5] == 'C'){
                Respond(_responseChannel, "clear dance all\n");
                HAL_FLASH_Unlock();             //解锁
                FLASH_Erase_Sector(11,FLASH_VOLTAGE_RANGE_3);
                HAL_FLASH_Lock();             //上锁
                Respond(_responseChannel, "clear dance all success\n");
                writeDanceInfo.currentStepNum = 0;
            }else {
                if(dummy.IsMoving() == true)
                {
                    Respond(_responseChannel, "err:robot is moving!");
                    //return;
                }

                bool valid = true;
                for (int j = 1; j <= 6; j++) {
                    if (dummy.currentJoints.a[j - 1] > dummy.motorJ[j]->angleLimitMax ||
                        dummy.currentJoints.a[j - 1] < dummy.motorJ[j]->angleLimitMin){
                        valid = false;
                        Respond(_responseChannel, "\nJ%d angle err:%.3f limit:%.3f - %.3f\n",j,dummy.SpeedLimit,dummy.motorJ[j]->angleLimitMin,dummy.motorJ[j]->angleLimitMax);
                    }
                }
                if(valid == false)
                    return;

                sscanf(_cmd, "[D]W %f", &dummy.SpeedLimit);
                angleStep.handConMode = HAND_CON_BY_ANGLE;

                uint32_t Stepoffset = DANCE_FLASH_ADDR+DANCE_HAND_SIZE+writeDanceInfo.currentStepNum*PER_STEP_SIZE;

                HAL_FLASH_Unlock();             //解锁
                Respond(_responseChannel,"\nwrite step:%d, ",writeDanceInfo.currentStepNum+1);
                for(int idx=0;idx<6;idx++){
                    HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, Stepoffset+ idx*4,*(uint32_t*)&dummy.currentJoints.a[idx]);
                    Respond(_responseChannel,"%.3f ",dummy.currentJoints.a[idx]);
                }
                Respond(_responseChannel,"\n");
                HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,Stepoffset + 4*6,*(uint32_t*)&dummy.SpeedLimit);
                HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,Stepoffset + 4*7,*(uint32_t*)&angleStep.handConMode);

                HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,Stepoffset + 4*8,*(uint32_t*)&dummy.hand->HandAngle);
                HAL_FLASH_Lock();             //上锁

                writeDanceInfo.currentStepNum++;

                Respond(_responseChannel, "add step,currentStepNum:%d\n",writeDanceInfo.currentStepNum);
            }
        }else if(_cmd[3] == 'D' && dummy.IsEnabled()){
            int Gap;
            float speedFac;
            sscanf(_cmd, "[D]D %d,%f", &Gap,&speedFac);
            danceHandInfo.StepSum = *(uint32_t*)DANCE_FLASH_ADDR;
            Respond(_responseChannel,"\nstep sum:%d Gap:%d speedFac:%.3f\n",danceHandInfo.StepSum,Gap,speedFac);
            for(int i = 0;i<danceHandInfo.StepSum;i++){
                uint32_t Stepoffset = DANCE_FLASH_ADDR + DANCE_HAND_SIZE + i * PER_STEP_SIZE;
                Respond(_responseChannel,"step:%d \n",i);
                for (int j = 0; j < 6; j++) {
                    angleStep.j[j] = *(float *)(Stepoffset+4*j);
                    Respond(_responseChannel,"%.3f,",angleStep.j[j]);
                }
                angleStep.speedLimit = *(float *)(Stepoffset+4*6);
                Respond(_responseChannel,"%.3f\n",angleStep.speedLimit);
                angleStep.handConMode = *(HandConMode *)(Stepoffset+4*7);
                Respond(_responseChannel,"%d ",angleStep.handConMode);
                angleStep.handConValue = *(float *)(Stepoffset+4*8);
                Respond(_responseChannel,"%.3f ",angleStep.handConValue);
                Respond(_responseChannel,"\n");


                for (int j = 0; j < 6; j++) {
                    dummy.targetJoints.a[j] = angleStep.j[j];
                }

                dummy.jointsStateFlag = 0b00000000;
                dummy.SetJointSpeed(angleStep.speedLimit*speedFac);
                dummy.MoveJ(angleStep.j[0], angleStep.j[1], angleStep.j[2], angleStep.j[3], angleStep.j[4], angleStep.j[5]);
                if(angleStep.handConMode == HAND_CON_BY_ANGLE)
                    dummy.hand->HandAngle = angleStep.handConValue;
                while(dummy.IsArrived(0.5) == false && dummy.IsMoving() == true){
                    dummy.MoveJoints(dummy.targetJoints);
                    dummy.hand->SetAngle(dummy.hand->HandAngle);
                    dummy.UpdateJointPose6D();
                    //Respond(*uart5StreamOutputPtr,">%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\n",angleStep.j[0], angleStep.j[1], angleStep.j[2], angleStep.j[3], angleStep.j[4], angleStep.j[5],angleStep.speedLimit*speedFac);
                    //Respond(_responseChannel,"step:%d doing",i);
                    //osDelay(10);
                }
                osDelay(Gap);
            }
        }else if(_cmd[3] == 'S') {
            HAL_FLASH_Unlock();             //解锁
            HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,DANCE_FLASH_ADDR,(uint32_t)writeDanceInfo.currentStepNum);
            HAL_FLASH_Lock();
            Respond(_responseChannel, "save dance,step sum:%d\n",writeDanceInfo.currentStepNum);

        }
    }else if(strstr(_cmd,"$cmd ") != NULL){
        _cmd = &_cmd[5];
        if(strstr(_cmd,"cali start") != NULL){
            HomeKeyFun(_cmd,_len);
        }else if(strstr(_cmd,"draw JP") != NULL){
            if(strstr(_cmd," En") != NULL){
                int Gap,gap2;
                int8_t argNum = sscanf(_cmd, "draw JP En Gap:%d %d", &Gap);
                if(argNum == 1 && Gap > 0 && Gap < 65535){
                    DrawJointAndPosOutputPtr = &_responseChannel;
                    DrawJointAndPosFlag = true;
                    DrawGapMs = Gap;
                }
            }else if(strstr(_cmd," Dis") != NULL){
                DrawJointAndPosFlag = false;
            }

        }else if(strstr(_cmd,"print bat msg ") != NULL){
            if(strstr(_cmd," En") != NULL){
                PrintBatMsgFlag = true;
                GettedChargePanelMsg = false;
            }else if(strstr(_cmd," Dis") != NULL){
                PrintBatMsgFlag = false;
            }

        }else if(strstr(_cmd,"reboot ") != NULL){
            if(strstr(_cmd,"reboot master") != NULL){
                HAL_NVIC_SystemReset();
            }else if(strstr(_cmd,"reboot M") != NULL){
                uint8_t id;
                int8_t argNum = sscanf(_cmd, "reboot M%d", &id);
                if(argNum == 1){
                    if(id >=0 && id <= 6){
                        dummy.initPose.a[id - 1] = dummy.currentJoints.a[id-1];
                        dummy.motorJ[id]->Reboot();
                    }
                }
            }
        }else if(strstr(_cmd,"find home") != NULL){
            if (strstr(_cmd," updata") != NULL) {
                FindHomeFlagUpdataFlag = true;
            }
            else
                FindHomeFlagUpdataFlag = false;

            if(dummy.HarVer == DUMMY)
                NeedFindHomeFlag = FIND_HOME_WITH_PHYSICAL_LIMIT;
            else if(dummy.HarVer == AR3 || dummy.HarVer == BATTLE_CHARGE_THERE_AXIS  || dummy.HarVer == MR6)
                NeedFindHomeFlag = FIND_HOME_WITH_PHYSICAL_KEY;
            else
                NeedFindHomeFlag = FIND_HOME_WITH_PHYSICAL_LIMIT;
        }
        else if(strstr(_cmd,"write data to flash") != NULL){
            initUserDataToFlash();
            Respond(*usbStreamOutputPtr, "write data to flash success\n");
            WriteLogToBuff(ROBOT_PARAMETER_ALTER_LOG,"write data to flash success");
        }else if(strstr(_cmd,"ask ") != NULL){
            if(strstr(_cmd," encoder pos") != NULL) {
                uint8_t id;
                int8_t argNum = sscanf(_cmd, "ask M%d encoder pos", &id);
                Respond(*usbStreamOutputPtr, "ask M%d encoder pos\n", id);
                if (argNum == 1)
                    dummy.motorJ[id]->FindHomePosWithVelocity(
                            FIND_INIT_POS_SPEED * (dummy.motorJ[id]->inverseDirection ? 1 : -1) *
                            (dummy.PHYSICAL_LIMIT_DIRT[id] ? -1 : 1), FindHomeFlagUpdataFlag, NeedFindHomeFlag);
            }

        }
        else if (strstr(_cmd, "print log ") != NULL){
            if(strstr(_cmd," lastest ") != NULL){
                uint32_t val;
                int8_t argNum = sscanf(_cmd, "print log lastest %d", &val);
                if(argNum == 1){
                    if(val > 0 && val <= 100000){
                        NeedPrintLogFlag = true;
                        if(val > LastLogNum)
                            val = LastLogNum;
                        PrintLogStartNum = LastLogNum - val;
                        PrintLogEndNum = LastLogNum - 1;
                    }
                }
            }else if(strstr(_cmd, "between ") != NULL){
                char *pStart = strstr(_cmd, "between ") + 8;
                char *pEnd = strstr(_cmd, "to ") + 3;
                TimesToTimestampMs(pStart,&PrintfLogStartTimes);
                TimesToTimestampMs(pEnd,&PrintfLogEndTimes);
                //Respond(*usbStreamOutputPtr,"times:%lld to %lld\n",PrintfLogStartTimes,PrintfLogEndTimes);
                NeedPrintrfLogWithTimesFlag = true;
            }
        }else if(strstr(_cmd,"set net time to ") != NULL){
            int year, month,day,hour,min,sec,week;
            int8_t argNum = sscanf(_cmd, "set net time to %d-%d-%d %d:%d:%d week:%d", &year, &month,&day,&hour,&min,&sec,&week);
            Respond(*usbStreamOutputPtr, "set net time to %d-%d-%d %d:%d:%d week:%d success", year,month,day,hour,min,sec,week);
            taskENTER_CRITICAL();
                I2C_DS3231_SetTime(year-2000, month,day,week,hour,min,sec);
            taskEXIT_CRITICAL();
        }else if (strstr(_cmd, "clean log") != NULL){
            for (int i = 0; i < W25QXX_PAGE_SUM; ++i) {
                W25QXX_Erase_Sector(i);
            }
            LastLogNum = 0;
            LogWriteNextAddr = 0;

        }
        else if (strstr(_cmd, "Gap con ") != NULL) {
            if(strstr(_cmd," on") != NULL){
                GapCon(true);
            }else if(strstr(_cmd," off") != NULL){
                GapCon(false);
            }
        }else if(strstr(_cmd,"pos ") != NULL){
            if((dummy.HarVer == BATTLE_CHARGE_THERE_AXIS || dummy.HarVer == MR6 || dummy.HarVer == IMU6) && strstr(_cmd,"EmptyUnit ") != NULL){
                if(strstr(_cmd,"set ") != NULL){
                    #if defined(BATTLE_CHARGE_THERE_AXIS_T)
                        if(dummy.HarVer == BATTLE_CHARGE_THERE_AXIS){
                            Pose3D_t pos;
                            int8_t argNum = sscanf(strstr(_cmd,"set "), "set %f,%f,%f", &pos.x,&pos.y,&pos.z);

                            //Respond(*usbStreamOutputPtr, "\ngetted, argNum:%d %.3f,%.3f,%.3f", argNum,pos.x,pos.y,pos.z);
                            if(argNum == 3){
                                if(pos.x > dummy.motorJ[2]->angleLimitMax || pos.x < dummy.motorJ[2]->angleLimitMin ||
                                   pos.y > dummy.motorJ[1]->angleLimitMax || pos.y < dummy.motorJ[1]->angleLimitMin ||
                                   pos.z > dummy.motorJ[3]->angleLimitMax || pos.z < dummy.motorJ[3]->angleLimitMin) {
                                    Respond(*usbStreamOutputPtr, "\nset EmptyUnit pos err, %.3f,%.3f,%.3f", pos.x,pos.y,pos.z);
                                    return;
                                }

                                Respond(*usbStreamOutputPtr, "\nset EmptyUnit pos ok, %.3f,%.3f,%.3f", pos.x,pos.y,pos.z);
                                PosOfEmptyUnit.x = pos.x;
                                PosOfEmptyUnit.y = pos.y;
                                PosOfEmptyUnit.z = pos.z;
                            }
                        }
                    #endif

                    #if defined(IMU6_T)
                        if(dummy.HarVer == IMU6){
                            Pose3D_t pos;
                            int8_t argNum = sscanf(strstr(_cmd,"set "), "set %f,%f,%f", &pos.x,&pos.y,&pos.z);

                            //Respond(*usbStreamOutputPtr, "\ngetted, argNum:%d %.3f,%.3f,%.3f", argNum,pos.x,pos.y,pos.z);
                            if(argNum == 3){
                                if(pos.x > dummy.motorJ[2]->angleLimitMax || pos.x < dummy.motorJ[2]->angleLimitMin ||
                                   pos.y > dummy.motorJ[1]->angleLimitMax || pos.y < dummy.motorJ[1]->angleLimitMin ||
                                   pos.z > dummy.motorJ[3]->angleLimitMax || pos.z < dummy.motorJ[3]->angleLimitMin) {
                                    Respond(*usbStreamOutputPtr, "\nset EmptyUnit pos err, %.3f,%.3f,%.3f", pos.x,pos.y,pos.z);
                                    return;
                                }

                                Respond(*usbStreamOutputPtr, "\nset EmptyUnit pos ok, %.3f,%.3f,%.3f", pos.x,pos.y,pos.z);
                                PosOfEmptyUnit.x = pos.x;
                                PosOfEmptyUnit.y = pos.y;
                                PosOfEmptyUnit.z = pos.z;
                            }
                        }
                    #endif

                    #if defined(MR6_T)
                        if(dummy.HarVer == MR6 && dummy.HardVerRobotNum == 2){
                            Pose6D_t pos;
                            int8_t argNum = sscanf(strstr(_cmd,"set "), "set %f,%f,%f,%f,%f,%f", &pos.x,&pos.y,&pos.z,&pos.a,&pos.b,&pos.c);

                            //Respond(*usbStreamOutputPtr, "\ngetted, argNum:%d %.3f,%.3f,%.3f,%.3f,%.3f,%.3f", argNum,pos.x,pos.y,pos.z,pos.a,pos.b,pos.c);
                            if(argNum == 6){
                                Respond(*usbStreamOutputPtr, "\nset EmptyUnit pos ok, %.3f,%.3f,%.3f,%.3f,%.3f,%.3f", pos.x,pos.y,pos.z,pos.a,pos.b,pos.c);
                                PosOfEmptyUnit.x = pos.x;
                                PosOfEmptyUnit.y = pos.y;
                                PosOfEmptyUnit.z = pos.z;
                                PosOfEmptyUnit.a = pos.a;
                                PosOfEmptyUnit.b = pos.b;
                                PosOfEmptyUnit.c = pos.c;
                            }
                        }
                    #endif

                }else{
                    #if defined(BATTLE_CHARGE_THERE_AXIS_T)
                        Respond(*usbStreamOutputPtr, "\nEmptyUnit pos:%.3f,%.3f,%.3f", PosOfEmptyUnit.x,PosOfEmptyUnit.y,PosOfEmptyUnit.z);
                    #endif

                    #if defined(IMU6_T)
                        Respond(*usbStreamOutputPtr, "\nEmptyUnit pos:%.3f,%.3f,%.3f", PosOfEmptyUnit.x,PosOfEmptyUnit.y,PosOfEmptyUnit.z);
                    #endif
                    #if defined(MR6_T)
                        Respond(*usbStreamOutputPtr, "\nEmptyUnit pos:%.3f,%.3f,%.3f,%.3f,%.3f,%.3f", PosOfEmptyUnit.x,PosOfEmptyUnit.y,PosOfEmptyUnit.z,PosOfEmptyUnit.a,PosOfEmptyUnit.b,PosOfEmptyUnit.c);
                    #endif

                }


            }
            else if(strstr(_cmd,"FullUnit ") != NULL){
                if(strstr(_cmd,"set ") != NULL){
                    #if defined(BATTLE_CHARGE_THERE_AXIS_T)
                        if(dummy.HarVer == BATTLE_CHARGE_THERE_AXIS){
                            Pose3D_t pos;
                            int8_t argNum = sscanf(strstr(_cmd,"set"), "set %f,%f,%f", &pos.x,&pos.y,&pos.z);

                            if(argNum == 3){
                                if(pos.x > dummy.motorJ[2]->angleLimitMax || pos.x < dummy.motorJ[2]->angleLimitMin ||
                                   pos.y > dummy.motorJ[1]->angleLimitMax || pos.y < dummy.motorJ[1]->angleLimitMin ||
                                   pos.z > dummy.motorJ[3]->angleLimitMax || pos.z < dummy.motorJ[3]->angleLimitMin) {
                                    Respond(*usbStreamOutputPtr, "\nset FullUnit pos err, %.3f,%.3f,%.3f", pos.x,pos.y,pos.z);
                                    return;
                                }

                                Respond(*usbStreamOutputPtr, "\nset FullUnit pos ok, %.3f,%.3f,%.3f", pos.x,pos.y,pos.z);
                                PosOfFullUnit.x = pos.x;
                                PosOfFullUnit.y = pos.y;
                                PosOfFullUnit.z = pos.z;
                            }
                        }
                    #endif

                    #if defined(IMU6_T)
                        if(dummy.HarVer == IMU6){
                            Pose3D_t pos;
                            int8_t argNum = sscanf(strstr(_cmd,"set"), "set %f,%f,%f", &pos.x,&pos.y,&pos.z);

                            if(argNum == 3){
                                if(pos.x > dummy.motorJ[2]->angleLimitMax || pos.x < dummy.motorJ[2]->angleLimitMin ||
                                   pos.y > dummy.motorJ[1]->angleLimitMax || pos.y < dummy.motorJ[1]->angleLimitMin ||
                                   pos.z > dummy.motorJ[3]->angleLimitMax || pos.z < dummy.motorJ[3]->angleLimitMin) {
                                    Respond(*usbStreamOutputPtr, "\nset FullUnit pos err, %.3f,%.3f,%.3f", pos.x,pos.y,pos.z);
                                    return;
                                }

                                Respond(*usbStreamOutputPtr, "\nset FullUnit pos ok, %.3f,%.3f,%.3f", pos.x,pos.y,pos.z);
                                PosOfFullUnit.x = pos.x;
                                PosOfFullUnit.y = pos.y;
                                PosOfFullUnit.z = pos.z;
                            }
                        }
                    #endif
                    #if defined(MR6_T)
                        if(dummy.HarVer == MR6  && dummy.HardVerRobotNum == 3){
                            Pose6D_t pos;
                        int8_t argNum = sscanf(strstr(_cmd,"set"), "set %f,%f,%f,%f,%f,%f", &pos.x,&pos.y,&pos.z,&pos.a,&pos.b,&pos.c);
                        if(argNum == 6){
                            Respond(*usbStreamOutputPtr, "\nset FullUnit pos ok, %.3f,%.3f,%.3f,%.3f,%.3f,%.3f", pos.x,pos.y,pos.z,pos.a,pos.b,pos.c);
                            PosOfFullUnit.x = pos.x;
                            PosOfFullUnit.y = pos.y;
                            PosOfFullUnit.z = pos.z;
                            PosOfFullUnit.a = pos.a;
                            PosOfFullUnit.b = pos.b;
                            PosOfFullUnit.c = pos.c;
                        }
                    }
                #endif

                }else{
                    #if defined(BATTLE_CHARGE_THERE_AXIS_T)
                        Respond(*usbStreamOutputPtr, "\nFullUnit pos:%.3f,%.3f,%.3f", PosOfFullUnit.x,PosOfFullUnit.y,PosOfFullUnit.z);
                    #endif
                    #if defined(IMU6_T)
                        Respond(*usbStreamOutputPtr, "\nFullUnit pos:%.3f,%.3f,%.3f", PosOfFullUnit.x,PosOfFullUnit.y,PosOfFullUnit.z);
                    #endif
                    #if defined(MR6_T)
                        Respond(*usbStreamOutputPtr, "\nFullUnit pos:%.3f,%.3f,%.3f,%.3f,%.3f,%.3f", PosOfFullUnit.x,PosOfFullUnit.y,PosOfFullUnit.z,PosOfFullUnit.a,PosOfFullUnit.b,PosOfFullUnit.c);
                    #endif
                }


            }
            #if defined(MR6_T)
                else if(strstr(_cmd,"FirBat ") != NULL){
                   if(strstr(_cmd,"set ") != NULL){
                       Pose6D_t pos;
                       int8_t argNum = sscanf(strstr(_cmd,"set"), "set %f,%f,%f,%f,%f,%f", &pos.x,&pos.y,&pos.z,&pos.a,&pos.b,&pos.c);
                       if(argNum == 6){
                           Respond(*usbStreamOutputPtr, "\nset FullUnit pos ok, %.3f,%.3f,%.3f,%.3f,%.3f,%.3f", pos.x,pos.y,pos.z,pos.a,pos.b,pos.c);
                           PosOfFirstBat.x = pos.x;
                           PosOfFirstBat.y = pos.y;
                           PosOfFirstBat.z = pos.z;
                           PosOfFirstBat.a = pos.a;
                           PosOfFirstBat.b = pos.b;
                           PosOfFirstBat.c = pos.c;
                       }

                   }else{
                       Respond(*usbStreamOutputPtr, "\nFirstBat pos:%.3f,%.3f,%.3f,%.3f,%.3f,%.3f", PosOfFirstBat.x,PosOfFirstBat.y,PosOfFirstBat.z,PosOfFirstBat.a,PosOfFirstBat.b,PosOfFirstBat.c);
                   }

                }
            #endif
            #ifdef BATTLE_CHARGE_THERE_AXIS_T
                else  if(dummy.HarVer == BATTLE_CHARGE_THERE_AXIS && strstr(_cmd,"ChargePanel ") != NULL){
                   if(strstr(_cmd,"set ") != NULL){
                       Pose3D_t pos;
                       float val[3];
                       char buff1[5],buff2[20];
                       for (int i = 0; i < 16; ++i) {
                           sprintf(buff1,"%d:",i+1);
                           sprintf(buff2,"%d:%%f,%%f,%%f",i+1);
                           //Respond(*usbStreamOutputPtr, "\nbuff2:%s", buff2);
                           int8_t argNum = sscanf(strstr(_cmd,buff1), buff2, &pos.x,&pos.y,&pos.z);

                           //Respond(*usbStreamOutputPtr, "\ngetted, argNum:%d %d:%.3f,%.3f,%.3f", argNum,i+1,pos.x,pos.y,pos.z);
                           if(argNum == 3){
                               if(pos.x > dummy.motorJ[2]->angleLimitMax || pos.x < dummy.motorJ[2]->angleLimitMin ||
                                  pos.y > dummy.motorJ[1]->angleLimitMax || pos.y < dummy.motorJ[1]->angleLimitMin ||
                                  pos.z > dummy.motorJ[3]->angleLimitMax || pos.z < dummy.motorJ[3]->angleLimitMin) {
                                   Respond(*usbStreamOutputPtr, "\nset ChargePanel pos err, %d:%.3f,%.3f,%.3f", i+1,pos.x,pos.y,pos.z);
                                   return;
                               }


                               Respond(*usbStreamOutputPtr, "\nset ChargePanel pos ok, %d:%.3f,%.3f,%.3f", i+1,pos.x,pos.y,pos.z);
                               ChargePanelPos[i+1].x = pos.x;
                               ChargePanelPos[i+1].y = pos.y;
                               ChargePanelPos[i+1].z = pos.z;
                           }
                       }
                   }else{
                       for (int i = 1; i <= 16; ++i) {
                           Respond(*usbStreamOutputPtr, "\n%d:%.3f,%.3f,%.3f", i,ChargePanelPos[i].x,ChargePanelPos[i].y,ChargePanelPos[i].z);
                       }

                   }


               }
            #endif
            #ifdef IMU6_T
                else  if(dummy.HarVer == IMU6 && strstr(_cmd,"ChargePanel ") != NULL){
                    if(strstr(_cmd,"set ") != NULL){
                        Pose3D_t pos;
                        float val[3];
                        char buff1[5],buff2[20];
                        for (int i = 0; i < 16; ++i) {
                            sprintf(buff1,"%d:",i+1);
                            sprintf(buff2,"%d:%%f,%%f,%%f",i+1);
                            //Respond(*usbStreamOutputPtr, "\nbuff2:%s", buff2);
                            int8_t argNum = sscanf(strstr(_cmd,buff1), buff2, &pos.x,&pos.y,&pos.z);

                            //Respond(*usbStreamOutputPtr, "\ngetted, argNum:%d %d:%.3f,%.3f,%.3f", argNum,i+1,pos.x,pos.y,pos.z);
                            if(argNum == 3){
                                if(pos.x > dummy.motorJ[2]->angleLimitMax || pos.x < dummy.motorJ[2]->angleLimitMin ||
                                   pos.y > dummy.motorJ[1]->angleLimitMax || pos.y < dummy.motorJ[1]->angleLimitMin ||
                                   pos.z > dummy.motorJ[3]->angleLimitMax || pos.z < dummy.motorJ[3]->angleLimitMin) {
                                    Respond(*usbStreamOutputPtr, "\nset ChargePanel pos err, %d:%.3f,%.3f,%.3f", i+1,pos.x,pos.y,pos.z);
                                    return;
                                }


                                Respond(*usbStreamOutputPtr, "\nset ChargePanel pos ok, %d:%.3f,%.3f,%.3f", i+1,pos.x,pos.y,pos.z);
                                ChargePanelPos[i+1].x = pos.x;
                                ChargePanelPos[i+1].y = pos.y;
                                ChargePanelPos[i+1].z = pos.z;
                            }
                        }
                    }else{
                        for (int i = 1; i <= 16; ++i) {
                            Respond(*usbStreamOutputPtr, "\n%d:%.3f,%.3f,%.3f", i,ChargePanelPos[i].x,ChargePanelPos[i].y,ChargePanelPos[i].z);
                        }

                    }


                }
            #endif
        }
    }else if(strstr(_cmd,"cali start") != NULL){
        HomeKeyFun(_cmd,_len);
    }
#endif

    if(_cmd[0] == '#'){
        if(strstr((char*)&_cmd[1],"Open IRC front")){
            irc_dir = 1;
            CheckPlaneData(208,0x0001,56,0x01,0x1c,irc_dir);
        }
        if(strstr((char*)&_cmd[1],"Open IRC behind")){
            irc_dir = 2;
            CheckPlaneData(208,0x0002,56,0x00,0x1c,irc_dir);
        }
        if(strstr((char*)&_cmd[1],"Open IRC left")){
            irc_dir = 3;
            CheckPlaneData(208,0x0004,56,0x00,0x1c, irc_dir );
        }
        if(strstr((char*)&_cmd[1],"Open IRC right")){
            irc_dir = 4;
            CheckPlaneData(208,0x0008,56,0x00,0x1c,irc_dir);
        }
        if(strstr((char*)&_cmd[1],"Close IRC")){
            //CheckPlaneData(208,0x0000,56,0x01,0x1c);
            irc_dir = 0;
            CheckPlaneData(208,0x0000,41,0x00,0x1c,irc_dir );
        }


    }

/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
}

void OnUsbAsciiCmd(const char *_cmd, size_t _len, StreamSink &_responseChannel) {
    ParseCommandFirst(_cmd, _len,_responseChannel);
}


void OnUart4AsciiCmd(const char *_cmd, size_t _len, StreamSink &_responseChannel) {
    ParseCommandFirst(_cmd, _len,_responseChannel);

/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
}


void OnUart5AsciiCmd(const char *_cmd, size_t _len, StreamSink &_responseChannel) {
    /*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/

/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
}

void OnUart1AsciiCmd(const char *_cmd, size_t _len, StreamSink &_responseChannel) {
    ParseCommandFirst(_cmd, _len,_responseChannel);
}